#include "thermal.h"
#include <Wire.h>
#include "mlx90640.h"
#include "displayer.h"
#include "button2.h"

#define LOG_ERROR   0
#define LOG_INFO    1
#define LOG_DBG     2

#define LOG_LEVEL   LOG_INFO

#if LOG_LEVEL >= LOG_INFO
#define LOG_I(fmt, ...)     Serial.printf("[%lu] %s: " fmt "\r\n", millis(), "I", ##__VA_ARGS__)
#else
#define LOG_I(fmt, ...)
#endif 

#if LOG_LEVEL >= LOG_DBG
#define LOG_D(fmt, ...)     Serial.printf("[%lu] %s: " fmt "\r\n", millis(), "D", ##__VA_ARGS__)
#else
#define LOG_D(fmt, ...) 
#endif 

#if LOG_LEVEL >= LOG_ERROR
#define LOG_E(fmt, ...)     Serial.printf("[%lu] %s: " fmt "\r\n", millis(), "E", ##__VA_ARGS__)
#else
#define LOG_E(fmt, ...) 
#endif

#define MATH_MAX(a, b) ({__typeof__(a) _a = (a); __typeof__(b) _b = (b); _a > _b ? _a : _b;})
#define MATH_MIN(a, b) ({__typeof__(a) _a = (a); __typeof__(b) _b = (b); _a < _b ? _a : _b;})

#define I2C_ADDR     0x33

#define  FPS2HZ   0x02
#define  FPS4HZ   0x03
#define  FPS8HZ   0x04
#define  FPS16HZ  0x05
#define  FPS32HZ  0x06

#define  RefreshRate FPS16HZ
#define  TA_SHIFT 8 //Default shift for MLX90640 in open air

#define BUTTON_1            0
#define BUTTON_2            35

static Button2 btn1(BUTTON_1);
static Button2 btn2(BUTTON_2);

#define DISP_TEMP_MIN       0
#define DISP_TEMP_MAX       100
#define DISP_TEMP_ADJ       5

#define DISP_TEMP_L_INDEX   0
#define DISP_TEMP_H_INDEX   1

typedef struct
{
    const char *name;
    int value;
} param_t;

static param_t disp_param[2] = 
{
    {
        .name = "tl",
        .value = 20,
    },

    {
        .name = "th",
        .value = 40,
    }
};

static int disp_param_index = 0;

static paramsMLX90640 mlx90640_param;

static rt_err_t mlx90640_init(void)
{
    int status;
    rt_err_t err = -RT_ERROR;
    uint16_t *eeMLX90640 = NULL;

    eeMLX90640 = new uint16_t[832];
    if(!eeMLX90640)
    {
        LOG_E("no memory for eeMLX90640");
        goto END;
    }

    LOG_D("mlx90640_init begin");
    status = MLX90640_SetRefreshRate(I2C_ADDR, RefreshRate);
    LOG_D("MLX90640_SetRefreshRate ret: %d", status);
    status = MLX90640_SetChessMode(I2C_ADDR);
    LOG_D("MLX90640_SetChessMode ret: %d", status);

    status = MLX90640_DumpEE(I2C_ADDR, eeMLX90640);
    if (status != 0)
    {
        LOG_E("load system parameters error with code:%d", status);
        goto END;
    }
    LOG_D("load system parameters succeed");

    status = MLX90640_ExtractParameters(eeMLX90640, &mlx90640_param);
    if (status != 0)
    {
        LOG_E("Parameter extraction failed with error code:%d",status);
        goto END;
    }
    LOG_D("Parameter extraction succeed");

    err = RT_EOK;
    LOG_D("mlx90640_init end");

END:
    if(eeMLX90640)
    {
        delete eeMLX90640;
        eeMLX90640 = NULL;
    }

    return err;
}

static void param_display(void)
{
    char buffer[128];

    for(int i = 0; i < sizeof(disp_param) / sizeof(disp_param[0]); i++)
    {
        snprintf(buffer, sizeof(buffer), "%s %s: %02d", 
            i == disp_param_index ? "*" : " ",
            disp_param[i].name,
            disp_param[i].value);
        sdisp_print(buffer, i);
    }
}

static rt_err_t button_init(void)
{
    btn1.setClickHandler([](Button2 & b) {
        // Serial.println("button1");
        param_t *param = &disp_param[disp_param_index];
        param->value -= DISP_TEMP_ADJ;
        param->value = MATH_MAX(param->value, DISP_TEMP_MIN);

        param_display();
    });

    btn2.setClickHandler([](Button2 & b) {
        // Serial.println("button2");

        param_t *param = &disp_param[disp_param_index];
        param->value += DISP_TEMP_ADJ;
        param->value = MATH_MIN(param->value, DISP_TEMP_MAX);

        param_display();
    });

    btn1.setDoubleClickHandler([](Button2 & b) {
        disp_param_index++;
        if(disp_param_index == sizeof(disp_param) / sizeof(disp_param[0]))
        {
            disp_param_index = 0;
        }
        param_display();
    });

    return RT_EOK;
}

static void button_loop(void)
{
    btn1.loop();
    btn2.loop();
}


#define INIT_CHECK(func)    \
if(func != RT_EOK)\
{\
    LOG_E(#func" fail");\
    return -RT_ERROR;\
}

rt_err_t thermal_cam_init(void)
{
    INIT_CHECK(mlx90640_init());
    INIT_CHECK(button_init());
    param_display();

    return RT_EOK;
}

static rt_err_t read_data(thermal_data_t *data)
{
    int status;
    static uint16_t frame[834];

    status = MLX90640_GetFrameData(I2C_ADDR, frame);
    if (status < 0)
    {
        if(status == -RT_EEMPTY)
        {
            return -RT_EEMPTY;
        }

        LOG_E("GetFrame Error: %d", status);
        return -RT_ERROR;
    }
    float vdd = MLX90640_GetVdd(frame, &mlx90640_param);
    float Ta = MLX90640_GetTa(frame, &mlx90640_param);

    float tr = Ta - TA_SHIFT; //Reflected temperature based on the sensor ambient temperature
    vdd = vdd;
    LOG_D("vdd: %.3fmV Tr: %2.2f", vdd, tr);

    float emissivity=0.95;
    MLX90640_CalculateTo(frame, &mlx90640_param, emissivity , tr, data->temp_img);
    return RT_EOK;
}

typedef struct
{
    float min_temp;
    float max_temp;
    float a, b, c, d;
} disp_param_t;

static void calculate_disp_param(thermal_data_t *data, disp_param_t *param)
{
    // param->min_temp = 255;
    // param->max_temp = 0;

    // for (int i = 0; i < THERMAL_IMG_SIZE; i++)
    // {
    //     param->min_temp = min(param->min_temp, data->temp_img[i]);
    //     param->max_temp = max(param->max_temp, data->temp_img[i]);
    // }

    param->min_temp = disp_param[DISP_TEMP_L_INDEX].value;
    param->max_temp = disp_param[DISP_TEMP_H_INDEX].value;

    param->a = param->min_temp + (param->max_temp - param->min_temp) * 0.2121f;
    param->b = param->min_temp + (param->max_temp - param->min_temp) * 0.3182f;
    param->c = param->min_temp + (param->max_temp - param->min_temp) * 0.4242f;
    param->d = param->min_temp + (param->max_temp - param->min_temp) * 0.8182f;
}

static uint16_t temp_to_color(disp_param_t *p, float val)
{
    /*
     pass in value and figure out R G B
     several published ways to do this I basically graphed R G B and developed simple linear equations
     again a 5-6-5 color display will not need accurate temp to R G B color calculation

     equations based on
     http://web-tech.ga-usa.com/2012/05/creating-a-custom-hot-to-cold-temperature-color-gradient-for-use-with-rrdtool/index.html

     */

    rt_uint8_t red = 0, green = 0, blue = 0;

    red = constrain(255.0f / (p->c - p->b) * val - ((p->b * 255.0f) / (p->c - p->b)), 0, 255);

    if ((val > p->min_temp) & (val < p->a))
    {
        green = constrain(255.0f / (p->a - p->min_temp) * val - (255.0f * p->min_temp) / (p->a - p->min_temp), 0, 255);
    }
    else if ((val >= p->a) & (val <= p->c))
    {
        green = 255;
    }
    else if (val > p->c)
    {
        green = constrain(255.0f / (p->c - p->d) * val - (p->d * 255.0f) / (p->c - p->d), 0, 255);
    }
    else if ((val > p->d) | (val < p->a))
    {
        green = 0;
    }

    if (val <= p->b)
    {
        blue = constrain(255.0f / (p->a - p->b) * val - (255.0f * p->b) / (p->a - p->b), 0, 255);
    }
    else if ((val > p->b) & (val <= p->d))
    {
        blue = 0;
    }
    else if (val > p->d)
    {
        blue = constrain(240.0f / (p->max_temp - p->d) * val - (p->d * 240.0f) / (p->max_temp - p->d), 0, 240);
    }

    // use the displays color mapping function to get 5-6-5 color palet (R=5 bits, G=6 bits, B-5 bits)
    return RGB888TO565(red, green, blue);
}

static void display_data(thermal_data_t *data)
{
    disp_param_t param;
    static rt_uint16_t rgb_buf[THERMAL_IMG_SIZE];

    calculate_disp_param(data, &param);

    uint16_t *rgb_line = rgb_buf;
    float *temp_line = data->temp_img;

    for(int i = 0; i < THERMAL_IMG_HEIGHT; i++)
    {
        uint16_t *rgb = rgb_line +  THERMAL_IMG_WIDTH - 1;
        float *temp = temp_line;

        for(int j = 0; j < THERMAL_IMG_WIDTH; j++)
        {
            *rgb = temp_to_color(&param, *temp);
            rgb--;
            temp++;
        }

        rgb_line += THERMAL_IMG_WIDTH;
        temp_line += THERMAL_IMG_WIDTH;
        
    }

    sdisp_show(rgb_buf, THERMAL_IMG_WIDTH, THERMAL_IMG_HEIGHT);

    float *temp = data->temp_img;
    data->center_temp = (temp[383 - 16] + temp[383 - 15] + temp[384 + 15] + temp[384 + 16]) / 4;
    LOG_D("center:%2.2f, max:%2.2f", data->center_temp, param.max_temp);
}

rt_err_t thermal_cam_loop(void)
{
    rt_err_t err;
    static thermal_data_t data = {0};

    button_loop();

    err = read_data(&data);
    if(err != RT_EOK)
    {
        return err;
    }

    display_data(&data);

    return RT_EOK;
}

static int i2c_read(uint8_t slave_addr, uint16_t start_addr, uint16_t size, uint16_t *data)
{
    uint8_t reg_addr[2];
    uint8_t *recv_buf = (uint8_t *)data;
    int cnt = 0;
    int ret;

    reg_addr[0] = start_addr >> 8;
    reg_addr[1] = start_addr & 0xff;

    Wire.beginTransmission(slave_addr);
    Wire.write(reg_addr, sizeof(reg_addr));
    ret = Wire.endTransmission(false);

    if(ret != 0)
    {
        return -RT_ERROR;
    }

    Wire.requestFrom((int)slave_addr, (int)size * 2, (int)true);
    while(Wire.available())
    {
        recv_buf[cnt++] = (uint8_t)Wire.read();
    }

    if(cnt != size * 2)
    {
        return -RT_ERROR;
    }

    uint8_t* bp = recv_buf;
    for (int i = 0; i < size * 2; i += 2)
    {
        uint8_t tmpbytelsb = bp[i + 1];
        bp[i + 1] = bp[i];
        bp[i] = tmpbytelsb;
    }

    return RT_EOK;
}

int MLX90640_I2CRead(uint8_t slave_addr, uint16_t start_addr, uint16_t size, uint16_t *data)
{
    uint16_t once_max = I2C_BUFFER_LENGTH / 2;
    uint16_t pos = 0;
    int ret;

    while(pos < size)
    {
        uint16_t once_size = MATH_MIN(once_max, size - pos);
        ret = i2c_read(slave_addr, start_addr + pos, once_size, data + pos);
        if(ret != RT_EOK)
        {
            return -RT_ERROR;
        }

        pos += once_size;
    }

    return RT_EOK;
}

int MLX90640_I2CWrite(uint8_t slave_addr, uint16_t reg_addr, uint16_t data)
{
    struct
    {
        rt_uint8_t reg_addr[2];
        rt_uint8_t cmd[2];
    } i2c_buf;

    int ret = -RT_ERROR;

    i2c_buf.reg_addr[0] = reg_addr >> 8;
    i2c_buf.reg_addr[1] = reg_addr & 0xff;
    i2c_buf.cmd[0] = data >> 8;
    i2c_buf.cmd[1] = data & 0xff;

    Wire.beginTransmission(slave_addr);
    ret = Wire.write((uint8_t *)&i2c_buf, sizeof(i2c_buf));
    Wire.endTransmission();

    return ret == sizeof(i2c_buf) ? RT_EOK : -RT_ERROR;
}

